#include "SteeringWander.h"

#include <cassert>

#include "ai/AiConstants.h"
#include "ai/steering/SteeringOutput.h"
#include "math/Point3.h"
#include "math/Utils.h"
#include "math/Vector3.h"
#include "physic/PhysicConstants.h"


WanderData::WanderData(const Point3* srcPos /*= nullptr*/,
                       const Vector3* srcLinearVel /*= nullptr*/,
                       const float* srcOrientation /*= nullptr*/)
                       : mSrcPos(srcPos)
                       , mSrcLinearVel(srcLinearVel)
                       , mSrcOrientation(srcOrientation)
                       , mWanderOrientation(0.0f)
{

}

void WanderData::set(const Point3& srcPos,
         const Vector3& srcLinearVel,
         const float& srcOrientation) 
{
    mSrcPos = &srcPos;
    mSrcLinearVel = &srcLinearVel;
    mSrcOrientation = &srcOrientation;
    mWanderOrientation = 0.0f;
}

void wander(WanderData * const data, 
            SteeringOutput * const outputs,
            const uint32_t numData)
{
    assert(data);
    assert(outputs);
    assert(numData > 0);

    // PARALLEL_FOR
    for (size_t i = 0; i < numData; ++i) {
        const Point3& srcPos = *data[i].mSrcPos;
        const Vector3& srcLinearVel = *data[i].mSrcLinearVel;
        const float& srcOrientation = *data[i].mSrcOrientation;
        float& wanderOrientation = data[i].mWanderOrientation;
        SteeringOutput& output = outputs[i];

        // Update wander orientation
        {
            const float rndBinomial = randomBinomial();
            wanderOrientation += rndBinomial * WANDER_RATE;
        }


        // Calculate the combined target orientation
        const float targetOrientation = wanderOrientation + srcOrientation;

        // Calculate the center of the wander circle
        Point3 targetPos;
        {
            Vector3 srcOrientationVector;
            unitVectorFromOrientation(srcOrientation, srcOrientationVector);
            srcOrientationVector *= WANDER_OFFSET;
            targetPos = srcPos + srcOrientationVector;
        }

        // Calculate the target location
        {
            Vector3 targetLinearVel;
            unitVectorFromOrientation(targetOrientation, targetLinearVel);
            targetLinearVel *= WANDER_RADIUS;
            targetPos += targetLinearVel;
        }

        // Computes unit vector 
        // from source position to target position
        Vector3 toTarget;
        toTarget.set(srcPos, targetPos);
        toTarget.normalize();

        // Set the linear velocity to be at full
        // speed in the direction of the orientation
        output.mLinearVel = toTarget * MAX_SPEED;
    }
}